//
// Created by jm on 2022/4/20.
//

#include "lidar_scan.h"
int LidarScan::Initialize(std::function<void(const std::vector<RangeData> &)> lidar_scan_callback,
                          const std::string &port,
                          unsigned int baud) {
  std::cout << "LidarScan::Initialize!"<<std::endl;
  lidar_.Initialize([this](const std::vector<RangeData> &data) {
    this->AddData(data);
  }, port, baud);
  lidar_scan_callback_ = lidar_scan_callback;

  init_flg_ = true;
  send_thread_ = std::thread(&LidarScan::Run, this);
  return 0;
}
int LidarScan::Release() {
  lidar_.Release();
  init_flg_ = false;
  send_thread_.join();
  return 0;
}
void LidarScan::AddData(const std::vector<RangeData> &points) {
  std::lock_guard<std::mutex> lk(mtx);
  for (const auto &point : points) {
    if ((point.ori_theta <= last_angle)&&point.ori_theta<0.03&&coll_data.size()>500) {
      {
        pub_data = coll_data;
        coll_data.clear();
      }
      mtx.unlock();
      condition_variable_.notify_all();
    }
      coll_data.emplace_back(point);
    last_angle = point.ori_theta;
  }
}
void LidarScan::Run() {
  std::cout << "LidarScan::Run!"<<std::endl;
  while (init_flg_) {
    {
      std::unique_lock<std::mutex> lk(mtx);
      condition_variable_.wait(lk, [this]() {
        return !pub_data.empty();
      });
      if (pub_data.size() < 550)
        std::cout<<camsense::LidarDevice::GetTimeStamp() << " size " << pub_data.size() << " " << pub_data.front().ori_theta << " " << pub_data.back().ori_theta
                  << std::endl;
//      else
//        std::cout << "size " << pub_data.size() << " " << pub_data.front().ori_theta << " " << pub_data.back().ori_theta
//                  << std::endl;
      pub_data.clear();
    }
    std::this_thread::sleep_for(std::chrono::microseconds(1));
  }
}
